#include "rclcpp/rclcpp.hpp"
#include "robot_interfaces/msg/robot_control.hpp"
#include "robot_interfaces/msg/robot_state.hpp"
#include "robot_interfaces/msg/robot_target.hpp"
#include "robot_interfaces/srv/set_mode.hpp"
#include <cmath>
#include <random>
#include <thread>
class RobotSolver : public rclcpp::Node {
public:
  RobotSolver(std::string name) : Node(name) {

    RCLCPP_INFO(this->get_logger(), "Hello, I am %s", name.c_str());
    this->declare_parameter<float>("bullet_speed", 20.0);
    this->declare_parameter<std::string>("gun_mode", "positive");
    target_subscriber_ =
        this->create_subscription<robot_interfaces::msg::RobotTarget>(
            "target", 10,
            std::bind(&RobotSolver::target_callback, this,
                      std::placeholders::_1));
    control_publisher_ =
        this->create_publisher<robot_interfaces::msg::RobotControl>("control",
                                                                    10);
    set_mode_server_ = this->create_service<robot_interfaces::srv::SetMode>(
        "set_mode", std::bind(&RobotSolver::handle_set_mode, this,
                              std::placeholders::_1, std::placeholders::_2));
  }

private:
  float base_x;
  float base_y;
  float target_x;
  float target_y;
  float target_v;
  float tan_theta_squ;
  float pre_target_x;
  float pre_target_y;
  float pre_theta;
  float time_step = 0.02;
  float correct_x = 0.0;
  float correct_y = 0.0;
  float gun_theta = 0.0;
  float bullet_speed=20.0;
  int the_first_time = 0;
  std::string command = "notfire";
  rclcpp::Subscription<robot_interfaces::msg::RobotTarget>::SharedPtr
      target_subscriber_;
  rclcpp::Publisher<robot_interfaces::msg::RobotControl>::SharedPtr
      control_publisher_;
  void timer_callback() {
    robot_interfaces::msg::RobotControl msg;
    msg.fire_theta = pre_theta;
    msg.fire_command = command;
    msg.gun_theta = gun_theta;
    control_publisher_->publish(msg);
  }
  rclcpp::Service<robot_interfaces::srv::SetMode>::SharedPtr set_mode_server_;
  void
  target_callback(const robot_interfaces::msg::RobotTarget::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Received target");
    float bullet_speed_;
    this->get_parameter("bullet_speed", bullet_speed_);
    this->bullet_speed = bullet_speed_;
    this->target_v = msg->target_world_v;
    this->target_x = msg->target_world_x;
    this->target_y = msg->target_world_y;
    this->tan_theta_squ = msg->target_tan_theta_squ;
    this->base_x = msg->base_world_x;
    this->base_y = msg->base_world_y;
    float p_n_x = (this->target_x - this->base_x) > 0 ? 1 : -1;
    float p_n_y = (this->target_y - this->base_y) > 0 ? 1 : -1;
    float distance = sqrt(pow(this->target_x - this->base_x, 2) +
                          pow(this->target_y - this->base_y, 2));
    float time = distance / this->bullet_speed;
    float pre_s = this->target_v * time;
    float cos_theta = std::sqrt(1.0 / (this->tan_theta_squ + 1.0));
    float sin_theta =
        std::sqrt(this->tan_theta_squ / (this->tan_theta_squ + 1.0));
    if (the_first_time == 0) {
      the_first_time = 1;
    } else {
      if (sqrt(pow(this->pre_target_x - this->target_x, 2) +
               pow(this->pre_target_y - this->target_y, 2)) < 0.25) {
        command = "fire";
      } else {
        correct_x = -(this->pre_target_x - this->target_x) / 2.68;
        correct_y = -(this->pre_target_y - this->target_y) / 2.68;
      }
    }
    float pre_target_x = this->target_x + p_n_x * pre_s * cos_theta + correct_x;
    float pre_target_y = this->target_y + p_n_y * pre_s * sin_theta + correct_y;
    float pre_tan_theta = (this->pre_target_y - this->base_y) /
                          (this->pre_target_x - this->base_x);
    this->pre_theta = std::atan(pre_tan_theta);
    timer_callback();
    if (this->gun_mode == "positive") {
      this->gun_theta = this->pre_theta;
    } else if (this->gun_mode == "passive") {
      float this_gun_theta = this->gun_theta;
      std::random_device rd;
      std::mt19937 gen(rd());
      std::uniform_real_distribution<> dis(-0.14753, 0.14753);
      while (1) {
        this->gun_theta += dis(gen);
        if (abs(this->gun_theta - this_gun_theta) > 0.14753) {
          this->gun_theta -= 0.08;
        }
      }
    }
    command = "notfire";
  }
  void handle_set_mode(
      const std::shared_ptr<robot_interfaces::srv::SetMode::Request> request,
      std::shared_ptr<robot_interfaces::srv::SetMode::Response> response) {

    std::string mode;
    mode = request->mode;
    RCLCPP_INFO(this->get_logger(), "Received set mode request: %s",
                mode.c_str());
    if (mode == "positive") {
      this->gun_theta = this->pre_theta;
    } else if (mode == "passive") {
      float this_gun_theta = this->gun_theta;
      std::random_device rd;
      std::mt19937 gen(rd());
      std::uniform_real_distribution<> dis(-0.14753, 0.14753);
      while (1) {
        this->gun_theta += dis(gen);
        if (abs(this->gun_theta - this_gun_theta) > 0.14753) {
          this->gun_theta -= 0.08;
        }
      }
    }
    response->success = true;
  }
  std::string gun_mode = "positive";
};
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RobotSolver>("robot_solver");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
